﻿#include "DlClient.h"
#include <QTcpSocket>
#include <QVariantList>
#include <QFile>
#include <QTimer>
#include <QDebug>
#include <QSharedMemory>
#include "JsonUtil.h"

typedef union _UDouble {
    double d;
    unsigned char c[8];
} UDouble;

double changeEndian2(double d)
{
    UDouble d1, d2, d3;
    memcpy(&d1.d, &d, 8);
    memcpy(&d2.d, &d, 8);
    for (int i=0; i<4; ++i)
    {
        d3.c[i] = d1.c[7-i];
        d3.c[i+4] = d2.c[3-i];
    }
    return d3.d;
}

DlClient::DlClient(int argc, char** argv, QObject* parent)
	: argc_(argc)
	, argv_(argv)
	, QObject(parent)
{
	m_socket = new QTcpSocket(this);
	m_ros_spin_thread = std::thread{ std::bind(&DlClient::rosSpinThread, this) };

	connect(m_socket, &QTcpSocket::readyRead, this, &DlClient::onSocketReadReady);
	connect(m_socket, &QTcpSocket::connected, this, &DlClient::onSocketConnected);
    connect(m_socket, &QTcpSocket::errorOccurred, this, &DlClient::onSocketError);
}

DlClient::~DlClient()
{
	rclcpp::shutdown();
}

void DlClient::connect2Host(const std::string& strIp, unsigned short usPort)
{
	m_strIp = QString::fromStdString(strIp);
	m_usPort = usPort;
    qDebug() << "in dlclient connetx2host: " << m_strIp << "  " << m_usPort;
	QMetaObject::invokeMethod(this, "connect2Host_p", Qt::QueuedConnection);
}

void DlClient::connect2Host_p()
{
    qDebug() << "starting to connect to host " << m_strIp << ":" << m_usPort;
	m_socket->connectToHost(QHostAddress(m_strIp), m_usPort);
}

void DlClient::onSocketConnected()
{
	qDebug() << "socket connected";
}

#define SOCK_SIZE_1 (2048)
static quint8 s_buf1[SOCK_SIZE_1] = { 0 };
static int s_index1 = 0;
void DlClient::onSocketReadReady()
{
	if (s_index1 >= SOCK_SIZE_1 - 200)
	{
		s_index1 = 0;
	}
	int nRead = m_socket->read((char*)s_buf1 + s_index1, 1024);
	//    qDebug() << "nRead1: " << nRead;
	s_index1 += nRead;
	//    int nParsed = parse1(s_buf1, s_index1);
	int nParsed = parse(s_buf1, s_index1);
	//    qDebug() << __FUNCTION__ << "nParsed2: " << nParsed;
	memmove(s_buf1, s_buf1 + nParsed, s_index1 - nParsed);
	s_index1 -= nParsed;
}

void DlClient::onSocketError(QAbstractSocket::SocketError e)
{
    qDebug() << "dlClient error: " << e;
}

void DlClient::rosSpinThread()
{
	rclcpp::init(argc_, argv_);
	m_robotCmdNode = std::make_shared<RobotCmdNode>(this);
	rclcpp::spin(m_robotCmdNode);
	rclcpp::shutdown();
}


int DlClient::parse(quint8* buf, int len)
{
    if (len < 54)
    {
        return 0;
    }

    int nStartIndex = 0;

    if (nStartIndex+54 > len)
    {
        qDebug() << "len is not 54";
        return nStartIndex;
    }


    while (buf[0+nStartIndex]*256+buf[1+nStartIndex] != 4545)
    {
        nStartIndex++;
        continue;
    }

    if (buf[2+nStartIndex]*256+buf[3+nStartIndex] != 1)
    {
        qDebug() << "6dof haven't been obtained: " << buf[2+nStartIndex]*256+buf[3+nStartIndex];
    }

    quint16 state = buf[2+nStartIndex]*256+buf[3+nStartIndex];

    if (buf[52+nStartIndex]*256+buf[53+nStartIndex] != 7878)
    {
        qDebug()<<"actual tail:"<<buf[52+nStartIndex]*256+buf[53+nStartIndex];
        return nStartIndex+54;
    }

    DLMsgRecv msg;
    memset(&msg, 0, sizeof(DLMsgRecv));
    memcpy(&msg, buf + 4 + nStartIndex, 8 * 6);

    double x = changeEndian2(msg.x);
    double y = changeEndian2(msg.y);
    double z = changeEndian2(msg.z);
    double rx = changeEndian2(msg.rx);
    double ry = changeEndian2(msg.ry);
    double rz = changeEndian2(msg.rz);

    m_robotCmdNode->pubDlMsg(x, y, z, rx, ry, rz, state);

    return nStartIndex+54;
}

